/*
 * seral_FourOne.c
 *
 *  Created on: 2023年4月9日
 *      Author: Harris
 */

#include "serial_FourOne.h"

uint8_t u_rx_buf_1[13] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; //串口1接收缓存
uint8_t u_rx_buf_2[44] = {0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02};			//串口2接收缓存
uint8_t u_rx_buf_3[44] = {0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03,0x03};			//串口3接收缓存
uint8_t u_rx_buf_4[44] = {0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04};      //串口4接收缓存
uint8_t u_tx_buf_5[8] = {0x01,0x03,0x00,0x00,0x00,0x04,0x44,0x09};			//串口5发送缓存，请求电压电流温度数据
uint8_t u_rx_buf_5[13] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
uint8_t u_rx_buf_6[9] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};			//串口6接收缓存

//modbus的crc校验 --这里作为传给飞控的数据校验
uint16_t calc_crc_modbus(uint8_t *buf, uint16_t len)
{
    uint16_t crc = 0xFFFF;
    for (uint16_t pos = 0; pos < len; pos++) {
        crc ^= (uint16_t) buf[pos]; // XOR byte into least sig. byte of crc
        for (uint8_t i = 8; i != 0; i--) { // Loop over each bit
            if ((crc & 0x0001) != 0) { // If the LSB is set
                crc >>= 1; // Shift right and XOR 0xA001
                crc ^= 0xA001;
            } else {
                // Else LSB is not set
                crc >>= 1; // Just shift right
            }
        }
    }
    return crc;
}

//二进制协议的校验--倾角传感器
uint16_t calc_crc_ASCII(const uint8_t *buf, uint16_t len)
{
		uint16_t poly = 0x8480;
    uint16_t crc = 0;
    uint8_t carry;
    uint8_t i_bits;
    uint16_t j;
    for (j = 0; j < len; j++)
    {
        crc = crc ^ buf[j];
        for (i_bits = 0; i_bits < 8; i_bits++)
        {
            carry = crc & 1;
            crc = crc / 2;
            if(carry)
            {
            	crc = crc ^ poly;
            }
        }
    }
    return crc;
}

void HAL_Delay(__IO uint32_t Delay)
{
  uint32_t tickstart = HAL_GetTick(); //获取tick值（毫秒�???
  uint32_t wait = Delay;

  /* Add a period to guarantee minimum wait */
  if (wait < HAL_MAX_DELAY)
  {
     wait++;//传参，延时的时间
  }3
  while((HAL_GetTick() - tickstart) < wait)
  {
  }
}

void send_to_pixhawk(void)
{
	int i = 0;
	int j = 0;
	int n = 0;
	int m = 0;
	int k = 0;
	uint16_t checksum_incli = 0;
	uint16_t checksum_encoder = 0;
	unsigned char send_inclination_8[53] = {0x03,0x03,0x18,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};    //串口8发送倾角缓存
	unsigned char send_encoder_7[11] = {0x01,0x03,0x06,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};

	//send_inclination_8更新boom的数据
	if(u_rx_buf_2[2] == 0x00)
	{
		//boom的roll，pitch，yaw角度信息
		for(i = 3; i <= 14; i++){
			send_inclination_8[i] = u_rx_buf_2[i+3];     //boom_Roll是buf_2的第7字节开始的四字节
		}
		//roll角速度信息
		for(i = 39; i <= 42; i++){
					send_inclination_8[i] = u_rx_buf_2[i-21];     //boom_Roll_velocity是buf_2的第19字节开始的四字节
		}
	}

	//send_inclination_8更新Forearm的数据
	if(u_rx_buf_3[2] == 0x00)
	{
		//forarm的roll，pitch，yaw角度信息
		for(i = 15; i <= 26; i++){
			send_inclination_8[i] = u_rx_buf_3[i-9];     //Forearm_roll是buf_3的第7字节开始的四字节
		}
		//roll角速度信息
		for(i = 43; i <= 46; i++){
			send_inclination_8[i] = u_rx_buf_3[i-25];     //Forearm_roll是buf_3的第19字节开始的四字节
		}
	}

	//send_inclination_8更新Bucket的数据
	if(u_rx_buf_4[2] == 0x00)
	{
		//bucket的roll，pitch，yaw角度信息
		for(i = 27; i <= 38; i++)
		{
			send_inclination_8[i] = u_rx_buf_4[i-21];     //Bucket_roll是buf_4的第7字节开始的四字节
		}
		//roll角速度信息
		for(i = 47; i <= 50; i++)
		{
			send_inclination_8[i] = u_rx_buf_4[i-29];     //Bucket_roll是buf_4的第19字节开始的四字节
		}
	}

	checksum_incli = calc_crc_modbus(send_inclination_8,51);

	send_inclination_8[51] = (uint8_t)(checksum_incli&0x00ff);

	send_inclination_8[52] = (uint8_t)(checksum_incli >> 8);


	//编码器更新数据
	if(u_rx_buf_6[0] == 0x01)
	{
		for(j = 3; j <= 6; j++)
		{
			send_encoder_7[j] = u_rx_buf_6[j];
		}
	}

	//电流数据更新
	if(u_rx_buf_5[0] == 0x01)
	{
		send_encoder_7[7] = u_rx_buf_5[5];
		send_encoder_7[8] = u_rx_buf_5[6];
	}

	checksum_encoder = calc_crc_modbus(send_encoder_7,9);

	send_encoder_7[9] = (uint8_t)(checksum_encoder & 0x00ff);

	send_encoder_7[10] = (uint8_t)(checksum_encoder >> 8);
	//温度传感器数据更新

	//发送倾角数据
	HAL_UART_Transmit_DMA(&huart8,send_inclination_8,sizeof(send_inclination_8));
	for(n = 0; n < 200; n++)
	{
		for(m = 0; m < 200; m++)
		{
			k = 0;
		}
	}
	//发送编码器数据
	HAL_UART_Transmit_DMA(&huart7,send_encoder_7,sizeof(send_encoder_7));

	for(n = 0; n < 100; n++)
	{
		for(m = 0; m < 50; m++)
		{
			k = 0;
		}
	}
}


void send_ele_to_pix()
{
	uint16_t checksumL = 0;
	uint16_t checksumH = 0;
	uint16_t checksum = 0;

	HAL_UART_Transmit_DMA(&huart5, u_tx_buf_5, sizeof(u_tx_buf_5));

	if(u_rx_buf_1[0] == 0x01 && u_rx_buf_1[1] == 0x03)
	{
		checksum = calc_crc_modbus(u_rx_buf_1, 11);
		checksumL = (uint8_t)(checksum&0x00ff);
		checksumH = (uint8_t)(checksum >> 8);
		if(checksumL == u_rx_buf_1[11] && checksumH == u_rx_buf_1[12])
		{
			HAL_UART_Transmit_DMA(&huart5, u_rx_buf_5, sizeof(u_rx_buf_5));
		}
	}
}

//红-橙   黑-红  黄-黄  绿-绿

// format of inclination sensor HDA436T serial packets send to ardupilot
//
// Data Bit             Definition      Description
// ------------------------------------------------
// byte 0               Frame header        0x03
// byte 1               Frame header        0x03
// byte 2               DATA_LENGTH         roll pitch yaw temperature these 4 data totle length of byte, default is 0x24
// byte 3               Boom_ROLL1_L        roll raw data 1 low 8 bits
// byte 4               Boom_ROLL1_H        roll raw data 1 high 8 bits
// byte 5               Boom_ROLL2_L        roll raw data 2 low 8 bits
// bute 6               Boom_ROLL2_H        roll raw data 2 high 8 bits
// byte 7               Boom_PITCH1_L       pitch raw data 1 low 8 bits
// byte 8               Boom_PITCH1_H       pitch raw data 1 high 8 bits
// byte 9               Boom_PITCH2_L       pitch raw data 2 low 8 bits
// bute 10              Boom_PITCH2_H       pitch raw data 2 high 8 bits
// byte 11              Boom_YAW1_L         yaw raw data 1 low 8 bits
// bute 12              Boom_YAW1_H         yaw raw data 1 high 8 bits
// byte 13              Boom_YAW2_L         yaw raw data 1 low 8 bits
// byte 14              Boom_YAW2_H         yaw raw data 1 high 8 bits
// byte 15              Forearm_ROLL1_L     roll raw data 1 low 8 bits
// byte 16              Forearm_ROLL1_H     roll raw data 1 high 8 bits
// byte 17              Forearm_ROLL2_L     roll raw data 2 low 8 bits
// byte 18              Forearm_ROLL2_H     roll raw data 2 high 8 bits
// byte 19              Forearm_PITCH1_L       pitch raw data 1 low 8 bits
// byte 20              Forearm_PITCH1_H       pitch raw data 1 high 8 bits
// byte 21              Forearm_PITCH2_L       pitch raw data 2 low 8 bits
// bute 22              Forearm_PITCH2_H       pitch raw data 2 high 8 bits
// byte 23              Forearm_YAW1_L      yaw raw data 1 low 8 bits
// byte 24              Forearm_YAW1_H      yaw raw data 1 high 8 bits
// byte 25              Forearm_YAW2_L      yaw raw data 1 low 8 bits
// byte 26              Forearm_YAW2_H      yaw raw data 1 high 8 bits
// byte 27              Bucket_ROLL1_L      roll raw data 1 low 8 bits
// byte 28              Bucket_ROLL1_H      roll raw data 1 high 8 bits
// byte 29              Bucket_ROLL2_L      roll raw data 2 low 8 bits
// byte 30              Bucket_ROLL2_H      roll raw data 2 high 8 bits
// byte 31              Bucket_PITCH1_L       pitch raw data 1 low 8 bits
// byte 32              Bucket_PITCH1_H       pitch raw data 1 high 8 bits
// byte 33              Bucket_PITCH2_L       pitch raw data 2 low 8 bits
// bute 34              Bucket_PITCH2_H       pitch raw data 2 high 8 bits
// byte 35              Bucket_YAW1_L       yaw raw data 1 low 8 bits
// byte 36              Bucket_YAW1_H       yaw raw data 1 high 8 bits
// byte 37              Bucket_YAW2_L       yaw raw data 1 low 8 bits
// byte 38              Bucket_YAW2_H       yaw raw data 1 high 8 bits
// byte 39              BOOM_VEL_ROLL1_L         velocity roll raw data 1 low 8 bits
// byte 40              BOOM_VEL_ROLL1_H         velocity roll raw data 1 high 8 bits
// byte 41              BOOM_VEL_ROLL2_L         velocity roll raw data 2 low 8 bits
// byte 42              BOOM_VEL_ROLL2_H         velocity roll raw data 2 high 8 bits
// byte 43              FORA_VEL_ROLL1_L
// byte 44              FORA_VEL_ROLL1_H
// byte 45              FORA_VEL_ROLL2_L
// byte 46              FORA_VEL_ROLL2_H
// byte 47              BUCK_VEL_ROLL1_L
// byte 48              BUCK_VEL_ROLL1_H
// byte 49              BUCK_VEL_ROLL2_L
// byte 50              BUCK_VEL_ROLL2_H
// byte 51              Checksum            low 8 bits of Checksum byte, sum of bytes 0 to bytes 16
// byte 52              Checksum            high 8 bits of Checksum byte, sum of bytes 0 to bytes 16
